#include "WPILib.h"

/**
 * Teleop Mode:
 * - Select between two different drive options depending upon Z-location of Joystick1
 * - When "Z-Up" (on Joystick1) provide "arcade drive" on Joystick1
 * - When "Z-Down" (on Joystick1) provide "tank drive" on Joystick1 and Joystick2
 * - Use Joystick buttons (on Joystick1 or Joystick2) to display the button number in binary on
 *   the solenoid LEDs (Note that this feature can be used to easily "map out" the buttons on a
 *   Joystick.  Note also that if multiple buttons are pressed simultaneously, a "15" is displayed
 *   on the solenoid LEDs to indicate that multiple buttons are pressed.)
 *
 * This code assumes the following connections:
 * - Driver Station:
 *   - USB 1 - The "right" joystick.  Used for either "arcade drive" or "right" stick for tank drive
 *   - USB 2 - The "left" joystick.  Used as the "left" stick for tank drive
 * 
 * - Robot:
 *   - Digital Sidecar 1:
 *     - PWM 1/3 - Connected to "left" drive motor(s)
 *     - PWM 2/4 - Connected to "right" drive motor(s)
 */

#include "EnTech2013Robot.h"
#include "EntechPorts.h"

#define AUTO_SUBSYSTEM 1
#define DRIVE_ON       1
#define DUMPER_ON      1
#define CLIMBER_ON     1
#define TILTER_ON      1
#define COMPRESSOR_ON  1


EnTech2013Robot::EnTech2013Robot(void)
    : m_robotDrive(NULL)
    , m_dumper(NULL)
    , m_tilter(NULL)
    , m_climber(NULL)
    , m_compressor(NULL)
{
    m_dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Constructor Started      ");
    m_dsLCD->UpdateLCD();
    
    // Initialize Subsystems variables - must be done before create any
    for (int i=0; i<c_maxSubsystems; i++) {
    	m_subsystems[i] = NULL;
    }
    m_subsystemCount = 0;

    // Acquire the Driver Station objects
    m_ds = DriverStation::GetInstance();
    m_dsLCD = DriverStationLCD::GetInstance();

    // Create a robot using standard left/right robot drive on PWMS 2,1
#if DRIVE_ON
    m_robotDrive = new Entech2013DriveSubsystem();
#endif
#if COMPRESSOR_ON
	m_compressor = new Compressor(c_pressureSwitch, c_compressorRelay);
#endif
#if DUMPER_ON
	m_dumper     = new DumperSubsystem();
#endif
#if TILTER_ON
	m_tilter     = new TilterSubsystem();
#endif
#if CLIMBER_ON
	m_climber    = new ClimberSubsystem();
#endif
	
    // Define joysticks being used at USB port #1 and USB port #2 on the Drivers Station
    m_leftStick = new Joystick(1);
    m_rightStick = new Joystick(2);

    m_dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Constructor Completed  (%d)  ", m_subsystemCount);
    m_dsLCD->UpdateLCD();
}
	
void EnTech2013Robot::RegisterSubsystem(EntechSubsystem *subsys)
{
#if AUTO_SUBSYSTEM
	if (subsys != NULL) {
		m_subsystems[m_subsystemCount] = subsys;
		m_subsystemCount++;
	}
#endif
}
	
	/********************************** RobotInit *************************************/

void EnTech2013Robot::RobotInit(void) 
{
    // Actions which would be performed once (and only once) upon initialization of the
    // robot would be put here.
    m_watchdog.SetExpiration(0.25);
    m_watchdog.SetEnabled(true);
    m_watchdog.Feed();
    
    // Create the operator interface buttons
    // *** Drive
    m_shiftHighButton        = new OperatorButton(c_ShiftHighJoystick, c_ShiftHighButton);
    m_shiftLowButton         = new OperatorButton(c_ShiftLowJoystick, c_ShiftLowButton);
    m_toggleDriveTypeButton  = new OperatorButton(c_ToggleDriveJoystick, c_ToggleDrive);
    m_forceDriveOnButton     = new OperatorButton(c_ForceDriveOnJoystick, c_ForceDriveOnButton);
    
    // *** Kicker/Dumper
	m_kickButton     = new OperatorButton(c_OperatorDumperKickPort);
	m_armButton      = new OperatorButton(c_OperatorDumperDumpPort);

	// Climber Buttons
	m_climberDeadman       = new OperatorButton(c_OperatorClimberDeadman);
	m_climberLeftForward   = new OperatorButton(c_OperatorLeftChainForward);
	m_climberLeftBackward  = new OperatorButton(c_OperatorLeftChainBackward); 
	m_climberRightForward  = new OperatorButton(c_OperatorRightChainForward);
	m_climberRightBackward = new OperatorButton(c_OperatorRightChainBackward);
	
	// *** Tilter
	m_tilterButton   = new OperatorButton(c_OperatorTilter);

	// Let the various subsystems do their magic

#if AUTO_SUBSYSTEM
    for (int i=0; i<m_subsystemCount; i++) {
    	m_subsystems[i]->DoRobotInit();
    }
#else
    if (m_robotDrive)
    	m_robotDrive->DoRobotInit();
    if (m_dumper)
    	m_dumper->DoRobotInit();
	if (m_tilter)
		m_tilter->DoRobotInit();
	if (m_climber)
		m_climber->DoRobotInit();
#endif
    if (m_compressor) {
    	m_compressor->Start();
    }
   
    m_dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "RobotInit() Completed  (%d)  ", m_subsystemCount);
    m_dsLCD->UpdateLCD();
}
	
//  ******  Disabled Routines *********************
void EnTech2013Robot::DisabledInit(void) 
{
#if AUTO_SUBSYSTEM
	for (int i=0; i<m_subsystemCount; i++) {
    	m_subsystems[i]->DoDisabledInit();
    }
#else
    if (m_robotDrive)
    	m_robotDrive->DoDisabledInit();
    if (m_dumper)
    	m_dumper->DoDisabledInit();
	if (m_tilter)
		m_tilter->DoDisabledInit();
	if (m_climber)
		m_climber->DoDisabledInit();
#endif
}

void EnTech2013Robot::DisabledPeriodic(void)  
{
    m_watchdog.Feed();

#if AUTO_SUBSYSTEM
    for (int i=0; i<m_subsystemCount; i++) {
    	m_subsystems[i]->DoDisabled();
    }
#else
    if (m_robotDrive)
    	m_robotDrive->DoDisabled();
    if (m_dumper)
    	m_dumper->DoDisabled();
	if (m_tilter)
		m_tilter->DoDisabled();
	if (m_climber)
		m_climber->DoDisabled();
#endif
}

//  ******  Teleop Routines *********************
void EnTech2013Robot::TeleopInit(void) 
{
#if AUTO_SUBSYSTEM
	for (int i=0; i<m_subsystemCount; i++) {
    	m_subsystems[i]->DoTeleopInit();
    }
#else
    if (m_robotDrive)
    	m_robotDrive->DoTeleopInit();
    if (m_dumper)
    	m_dumper->DoTeleopInit();
	if (m_tilter)
		m_tilter->DoTeleopInit();
	if (m_climber)
		m_climber->DoTeleopInit();
#endif
}

void EnTech2013Robot::TeleopPeriodic(void) 
{
    m_watchdog.Feed();

    // Commands for the Drive
    if (m_robotDrive) {
	    if (m_shiftHighButton->Get() == OperatorButton::kJustPressed) {
	    	m_robotDrive->ShiftHigh();
	    }
	    if (m_shiftLowButton->Get() == OperatorButton::kJustPressed) {
	    	m_robotDrive->ShiftLow();
	    }
	    if (m_toggleDriveTypeButton->Get() == OperatorButton::kJustPressed) {
	    	m_robotDrive->ToggleDriveType();
	    }
	    if (m_forceDriveOnButton->Get() == OperatorButton::kJustPressed) {
	    	m_robotDrive->ForceOn();
	    }
    }
    
    // Commands for the Dumper
    if (m_dumper) {
		if (m_kickButton->GetBool()) {
			m_dumper->SetPosition(DumperSubsystem::Low);
		}
		if (m_armButton->GetBool()) {
			m_dumper->SetPosition(DumperSubsystem::High);
		}
		if (!m_kickButton->GetBool() && !m_armButton->GetBool()) {
			m_dumper->SetPosition(DumperSubsystem::Start);
		}
    }
	
	// Commands for the Tilter
    if (m_tilter) {
		if (m_tilterButton->GetBool()) {
			m_tilter->SetPosition(TilterSubsystem::kTilted);
		} else {
			m_tilter->SetPosition(TilterSubsystem::kFlat);
		}
    }
	
	// Commands for the Climber
	if (m_climber) {
		m_climber->TeleopInputs(m_climberDeadman->GetBool(),
				m_climberLeftForward->GetBool(),m_climberLeftBackward->GetBool(), 
				m_climberRightForward->GetBool(),m_climberRightBackward->GetBool() ); 
	}

	// Let the sub-systems do their work
#if AUTO_SUBSYSTEM
	for (int i=0; i<m_subsystemCount; i++) {
    	m_subsystems[i]->DoTeleop();
    }
#else
    if (m_robotDrive)
    	m_robotDrive->DoTeleop();
    if (m_dumper)
    	m_dumper->DoTeleop();
	if (m_tilter)
		m_tilter->DoTeleop();
	if (m_climber)
		m_climber->DoTeleop();
#endif
    m_dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Teleop Loop Completed  (%d)  ", m_subsystemCount);
    m_dsLCD->UpdateLCD();
}

// ******  Autonomous Routines ***************
void EnTech2013Robot::AutonomousInit(void)
{
	DigitalInput dig1(c_autonomousStartPosition1);
	DigitalInput dig2(c_autonomousStartPosition2);
	DigitalInput dig3(c_autonomousExitPath);
	AnalogChannel ana1(c_autonomousDelay);
	
	m_newState = true;
	m_autoTimer = new Timer();
	m_startPosition = dig1.Get() + dig2.Get();
	m_exitPath = dig3.Get();
	if ( 0 == m_startPosition ) {
		m_waitDelay = ana1.GetVoltage() * c_wait0;
		m_autoState = kWait0;
	} else if ( 1 == m_startPosition ) {
		m_waitDelay = ana1.GetVoltage() * c_wait1;
		m_autoState = kWait1;
	} else {
		m_waitDelay = ana1.GetVoltage() * c_wait2;
		m_autoState = kWait2;
	} 
	
#if AUTO_SUBSYSTEM
	for (int i=0; i<m_subsystemCount; i++) {
    	m_subsystems[i]->DoAutonomousInit();
    }
#else
    if (m_robotDrive)
    	m_robotDrive->DoAutonomousInit();
    if (m_dumper)
    	m_dumper->DoAutonomousInit();
	if (m_tilter)
		m_tilter->DoAutonomousInit();
	if (m_climber)
		m_climber->DoAutonomousInit();
#endif
}

// TODO: Need a method to store autonomous parameters for the times, distances, speeds, 
//    for each starting position.
void EnTech2013Robot::AutonomousPeriodic(void) 
{
    m_watchdog.Feed();
    
    switch (m_autoState) {
    case kWait0:
    	if (m_newState) {
    		m_autoTimer->Reset();
    		m_autoTimer->Start();
    		m_newState = false;
    	}
    	// Reached exit criteria (1s delay), next state
    	if (m_autoTimer->Get() > m_waitDelay) {
    		m_autoTimer->Stop();
    		m_newState = true;
    		m_autoState = kP0Drive1;
    	}
    	break;
    case kWait1:
    	if (m_newState) {
    		m_autoTimer->Reset();
    		m_autoTimer->Start();
    		m_newState = false;
    	}
    	// Reached exit criteria (1s delay), next state
    	if (m_autoTimer->Get() > m_waitDelay) {
    		m_autoTimer->Stop();
    		m_newState = true;
    		m_autoState = kP1Drive1;
    	}
    	break;
    case kWait2:
    	if (m_newState) {
    		m_autoTimer->Reset();
    		m_autoTimer->Start();
    		m_newState = false;
    	}
    	// Reached exit criteria (1s delay), next state
    	if (m_autoTimer->Get() > m_waitDelay) {
    		m_autoTimer->Stop();
    		m_newState = true;
    		m_autoState = kP2Drive1;
    	}
    	break;
    case kP0Drive1:
    	if (m_newState) {
    		m_robotDrive->StraightDistance(-18.0, 0.25);
    		m_newState = false;
    	}
    	if (m_robotDrive->CommandDone()) {
    		m_newState = true;
    		m_autoState = kP0Drive2;
    	}
    	break;
    case kP0Drive2:
    	if (m_newState) {
    		m_robotDrive->Turn(90.0);
    		m_newState = false;
    	}
    	if (m_robotDrive->CommandDone()) {
    		m_newState = true;
    		m_autoState = kP0Drive3;
    	}
    	break;
    case kP0Drive3:
    	if (m_newState) {
    		m_robotDrive->StraightDistance(-72.0, 0.5);
    		m_newState = false;
    	}
    	if (m_robotDrive->CommandDone()) {
    		m_newState = true;
    		m_autoState = kP0Drive4;
    	}
    	break;
    case kP0Drive4:
    	if (m_newState) {
    		m_robotDrive->Turn(-45.0);
    		m_newState = false;
    	}
    	if (m_robotDrive->CommandDone()) {
    		m_newState = true;
    		m_autoState = kP0Drive5;
    	}
    	break;
    case kP0Drive5:
    	if (m_newState) {
    		m_robotDrive->StraightDistance(-24.0, 0.25);
    		m_newState = false;
    	}
    	if (m_robotDrive->CommandDone()) {
    		m_newState = true;
    		m_autoState = kP0Drive6;
    	}
    	break;
    case kP0Drive6:
    	if (m_newState) {
    		m_robotDrive->StraightTime(1.0, 0.25);
    		m_newState = false;
    	}
    	if (m_robotDrive->CommandDone()) {
    		m_newState = true;
    		m_autoState = kKick;
    	}
    	break;
    // ***** Position 1 Drive
    case kP1Drive1:
    	if (m_newState) {
    		m_robotDrive->StraightDistance(-18.0, 0.4);
    		m_newState = false;
    	}
    	if (m_robotDrive->CommandDone()) {
    		m_newState = true;
    		m_autoState = kP1Drive2;
    	}
    	break;
    case kP1Drive2:
    	if (m_newState) {
    		m_robotDrive->Turn(90.0);
    		m_newState = false;
    	}
    	if (m_robotDrive->CommandDone()) {
    		m_newState = true;
    		m_autoState = kP1Drive3;
    	}
    	break;
    case kP1Drive3:
    	if (m_newState) {
    		m_robotDrive->StraightDistance(-36.0, 0.4);
    		m_newState = false;
    	}
    	if (m_robotDrive->CommandDone()) {
    		m_newState = true;
    		m_autoState = kP1Drive4;
    	}
    	break;
    case kP1Drive4:
    	if (m_newState) {
    		m_robotDrive->Turn(90.0);
    		m_newState = false;
    	}
    	if (m_robotDrive->CommandDone()) {
    		m_newState = true;
    		m_autoState = kP1Drive5;
    	}
    	break;
    case kP1Drive5:
    	if (m_newState) {
    		m_robotDrive->StraightDistance(-18.0, 0.4);
    		m_newState = false;
    	}
    	if (m_robotDrive->CommandDone()) {
    		m_newState = true;
    		m_autoState = kP0Drive3;
    	}
    	break;
    // **** Position 2 Drive
    case kP2Drive1:
    	if (m_newState) {
    		m_robotDrive->StraightDistance(-18.0, 0.25);
    		m_newState = false;
    	}
    	if (m_robotDrive->CommandDone()) {
    		m_newState = true;
    		m_autoState = kP2Drive2;
    	}
    	break;
    case kP2Drive2:
    	if (m_newState) {
    		m_robotDrive->Turn(-90.0);
    		m_newState = false;
    	}
    	if (m_robotDrive->CommandDone()) {
    		m_newState = true;
    		m_autoState = kP2Drive3;
    	}
    	break;
    case kP2Drive3:
    	if (m_newState) {
    		m_robotDrive->StraightDistance(-72.0, 0.5);
    		m_newState = false;
    	}
    	if (m_robotDrive->CommandDone()) {
    		m_newState = true;
    		m_autoState = kP2Drive4;
    	}
    	break;
    case kP2Drive4:
    	if (m_newState) {
    		m_robotDrive->Turn(-90.0);
    		m_newState = false;
    	}
    	if (m_robotDrive->CommandDone()) {
    		m_newState = true;
    		m_autoState = kP2Drive5;
    	}
    	break;
    case kP2Drive5:
    	if (m_newState) {
    		m_robotDrive->StraightDistance(-72.0, 0.25);
    		m_newState = false;
    	}
    	if (m_robotDrive->CommandDone()) {
    		m_newState = true;
    		m_autoState = kP2Drive6;
    	}
    	break;
    case kP2Drive6:
    	if (m_newState) {
    		m_robotDrive->Turn(45.0);
    		m_newState = false;
    	}
    	if (m_robotDrive->CommandDone()) {
    		m_newState = true;
    		m_autoState = kP2Drive7;
    	}
    	break;
    case kP2Drive7:
    	if (m_newState) {
    		m_robotDrive->StraightDistance(-18.0, 0.25);
    		m_newState = false;
    	}
    	if (m_robotDrive->CommandDone()) {
    		m_newState = true;
    		m_autoState = kP0Drive6;
    	}
    	break;
    case kKick:
    	if (m_newState) {
    		m_autoTimer->Reset();
    		m_autoTimer->Start();
    		m_newState = false;
    		if (m_dumper)
    			m_dumper->SetPosition(DumperSubsystem::Low);
    	}
    	// Reached exit criteria (1s delay), next state
    	if (m_autoTimer->Get() > 1.0) {
    		m_autoTimer->Stop();
    		m_newState = true;
    		m_autoState = kKickDown;
    	}
    	break;
    case kKickDown:
    	if (m_newState) {
    		m_autoTimer->Reset();
    		m_autoTimer->Start();
    		m_newState = false;
    		m_dumper->SetPosition(DumperSubsystem::Start);
    	}
    	// Reached exit criteria (1s delay), next state
    	if (m_autoTimer->Get() > 1.0) {
    		m_autoTimer->Stop();
    		m_newState = true;
    		if (m_exitPath == 0) {
    			m_autoState = kE0Drive1;
    		} else {
    			m_autoState = kE1Drive1;    			
    		}
    	}
    	break;
    case kE0Drive1:
    	if (m_newState) {
    		m_robotDrive->StraightDistance(12.0, 0.4);
    		m_newState = false;
    	}
    	if (m_robotDrive->CommandDone()) {
    		m_newState = true;
    		m_autoState = kE0Drive2;
    	}
    	break;
    case kE0Drive2:
    	if (m_newState) {
    		m_robotDrive->Turn(45.0);
    		m_newState = false;
    	}
    	if (m_robotDrive->CommandDone()) {
    		m_newState = true;
    		m_autoState = kE0Drive3;
    	}
    	break;
    case kE0Drive3:
    	if (m_newState) {
    		m_robotDrive->StraightDistance(72.0, 0.4);
    		m_newState = false;
    	}
    	if (m_robotDrive->CommandDone()) {
    		m_newState = true;
    		m_autoState = kDone;
    	}
    	break;
    case kE1Drive1:
    	if (m_newState) {
    		m_robotDrive->StraightDistance(12.0, 0.4);
    		m_newState = false;
    	}
    	if (m_robotDrive->CommandDone()) {
    		m_newState = true;
    		m_autoState = kE1Drive2;
    	}
    	break;
    case kE1Drive2:
    	if (m_newState) {
    		m_robotDrive->Turn(-45.0);
    		m_newState = false;
    	}
    	if (m_robotDrive->CommandDone()) {
    		m_newState = true;
    		m_autoState = kE1Drive3;
    	}
    	break;
    case kE1Drive3:
    	if (m_newState) {
    		m_robotDrive->StraightDistance(60.0, 0.4);
    		m_newState = false;
    	}
    	if (m_robotDrive->CommandDone()) {
    		m_newState = true;
    		m_autoState = kDone;
    	}
    	break;
    case kDone:
		m_robotDrive->Stop();
    	break;
    }
    
    // Let the subsystems do their work
#if AUTO_SUBSYSTEM
	for (int i=0; i<m_subsystemCount; i++) {
    	m_subsystems[i]->DoAutonomous();
    }
#else
    if (m_robotDrive)
    	m_robotDrive->DoAutonomous();
    if (m_dumper)
    	m_dumper->DoAutonomous();
	if (m_tilter)
		m_tilter->DoAutonomous();
	if (m_climber)
		m_climber->DoAutonomous();
#endif
}

//  ******  Disabled Routines *********************
void EnTech2013Robot::TestInit(void) 
{
#if AUTO_SUBSYSTEM
	for (int i=0; i<m_subsystemCount; i++) {
    	m_subsystems[i]->DoTestInit();
    	m_subsystems[i]->TestModeEnabled(false);
    }
	m_testNumber = 0;
	m_testSubsys = 0;
	if (m_subsystems[m_testSubsys])
		m_subsystems[m_testSubsys]->TestModeEnabled(true);
#else
    if (m_robotDrive) {
    	m_robotDrive->DoTestInit();
    	m_robotDrive->TestModeEnabled(true);
    }
    if (m_dumper) {
    	m_dumper->DoTestInit();
    	m_dumper->TestModeEnabled(false);
    }
	if (m_tilter) {
		m_tilter->DoTestInit();
    	m_tilter->TestModeEnabled(false);
	}
	if (m_climber) {
		m_climber->DoTestInit();
    	m_climber->TestModeEnabled(false);
	}
#endif
	
    m_dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Test Mode                ");
    m_dsLCD->UpdateLCD();
}

void EnTech2013Robot::TestPeriodic(void)
{
    m_watchdog.Feed();

    if (m_leftStick->GetTrigger()) {
    	m_subsystems[m_testSubsys]->TestModeEnabled(false);
    	m_testSubsys = (m_testSubsys + 1) % m_subsystemCount;
    	m_subsystems[m_testSubsys]->TestModeEnabled(true);
    }
    if (m_leftStick->GetRawButton(2)) {
    	m_subsystems[m_testSubsys]->SetTestNumber(m_subsystems[m_testSubsys]->GetTestNumber() - 1);
    }
    if (m_leftStick->GetRawButton(3)) {
    	m_subsystems[m_testSubsys]->SetTestNumber(m_subsystems[m_testSubsys]->GetTestNumber() + 1);
    }

#if AUTO_SUBSYSTEM
    for (int i=0; i<m_subsystemCount; i++) {
    	m_subsystems[i]->DoTest();
    }
#else
    if (m_robotDrive) {
    	m_robotDrive->DoTest();
    }
    if (m_dumper) {
    	m_dumper->DoTest();
    }
	if (m_tilter) {
		m_tilter->DoTest();
	}
	if (m_climber) {
		m_climber->DoTest();
	}
#endif

	m_dsLCD->Printf(DriverStationLCD::kUser_Line5, 1, "Test Subsystem: %s  ",m_subsystems[m_testSubsys]->GetName().c_str());
    m_dsLCD->Printf(DriverStationLCD::kUser_Line6, 1, "Test Number: %d  ",m_subsystems[m_testSubsys]->GetTestNumber());
    m_dsLCD->UpdateLCD();
}

START_ROBOT_CLASS(EnTech2013Robot);
